#include "Mat4.h"
#include <memory>
#include <sstream>
#include "Mathf.h"
#include "CommonD3d.h"

float gTmpArr[16] = {0};
const auto gByteCount = sizeof(float) * 16;

Mat4::Mat4()
{
	memset(m, 0, gByteCount);
	m[0] = m[5] = m[10] = m[15] = 1;
}

Mat4::Mat4(const Vec4& column0, const Vec4& column1, const Vec4& column2, const Vec4& column3)
{
	m[0] = column0.x;
	m[1] = column0.y;
	m[2] = column0.z;
	m[3] = column0.w;

	m[4] = column1.x;
	m[5] = column1.y;
	m[6] = column1.z;
	m[7] = column1.w;

	m[8] = column2.x;
	m[9] = column2.y;
	m[10] = column2.z;
	m[11] = column2.w;

	m[12] = column3.x;
	m[13] = column3.y;
	m[14] = column3.z;
	m[15] = column3.w;
}

Mat4::Mat4(const float* arr)
{
	memcpy_s(m, gByteCount, arr, gByteCount);
}

Mat4& Mat4::Identity()
{
	memset(m, 0, gByteCount);
	m[0] = m[5] = m[10] = m[15] = 1;
	return *this;
}

Mat4& Mat4::Multiply(const Mat4& other)
{
	memcpy_s(gTmpArr, gByteCount, m, gByteCount);
	 
	const float* m2 = other.m;

	m[0] = gTmpArr[0] * m2[0] + gTmpArr[4] * m2[1] + gTmpArr[8] * m2[2] + gTmpArr[12] * m2[3];
	m[1] = gTmpArr[1] * m2[0] + gTmpArr[5] * m2[1] + gTmpArr[9] * m2[2] + gTmpArr[13] * m2[3];
	m[2] = gTmpArr[2] * m2[0] + gTmpArr[6] * m2[1] + gTmpArr[10] * m2[2] + gTmpArr[14] * m2[3];
	m[3] = gTmpArr[3] * m2[0] + gTmpArr[7] * m2[1] + gTmpArr[11] * m2[2] + gTmpArr[15] * m2[3];

	m[4] = gTmpArr[0] * m2[4] + gTmpArr[4] * m2[5] + gTmpArr[8] *  m2[6] + gTmpArr[12] * m2[7];
	m[5] = gTmpArr[1] * m2[4] + gTmpArr[5] * m2[5] + gTmpArr[9] *  m2[6] + gTmpArr[13] * m2[7];
	m[6] = gTmpArr[2] * m2[4] + gTmpArr[6] * m2[5] + gTmpArr[10] * m2[6] + gTmpArr[14] * m2[7];
	m[7] = gTmpArr[3] * m2[4] + gTmpArr[7] * m2[5] + gTmpArr[11] * m2[6] + gTmpArr[15] * m2[7];

	m[8] = gTmpArr[0] *  m2[8] + gTmpArr[4] * m2[9] + gTmpArr[8] *  m2[10] + gTmpArr[12] * m2[11];
	m[9] = gTmpArr[1] *  m2[8] + gTmpArr[5] * m2[9] + gTmpArr[9] *  m2[10] + gTmpArr[13] * m2[11];
	m[10] = gTmpArr[2] * m2[8] + gTmpArr[6] * m2[9] + gTmpArr[10] * m2[10] + gTmpArr[14] * m2[11];
	m[11] = gTmpArr[3] * m2[8] + gTmpArr[7] * m2[9] + gTmpArr[11] * m2[10] + gTmpArr[15] * m2[11];

	m[12] = gTmpArr[0] * m2[12] + gTmpArr[4] * m2[13] + gTmpArr[8] *  m2[14] + gTmpArr[12] * m2[15];
	m[13] = gTmpArr[1] * m2[12] + gTmpArr[5] * m2[13] + gTmpArr[9] *  m2[14] + gTmpArr[13] * m2[15];
	m[14] = gTmpArr[2] * m2[12] + gTmpArr[6] * m2[13] + gTmpArr[10] * m2[14] + gTmpArr[14] * m2[15];
	m[15] = gTmpArr[3] * m2[12] + gTmpArr[7] * m2[13] + gTmpArr[11] * m2[14] + gTmpArr[15] * m2[15];

	return *this;
}

Vec3 Mat4::TransformPoint(const Vec3& point)
{
	float x = m[0] * point.x + m[4] * point.y + m[8] * point.z + m[12];
	float y = m[1] * point.x + m[5] * point.y + m[9] * point.z + m[13];
	float z = m[2] * point.x + m[6] * point.y + m[10] * point.z + m[14];
	float w = m[3] * point.x + m[7] * point.y + m[11] * point.z + m[15];
	return Vec3(x / w,y / w,z / w);
}

Vec3 Mat4::TransformDirection(const Vec3& dir)
{
	float x = m[0] * dir.x + m[4] * dir.y + m[8] * dir.z ;
	float y = m[1] * dir.x + m[5] * dir.y + m[9] * dir.z ;
	float z = m[2] * dir.x + m[6] * dir.y + m[10] * dir.z;
	float w = m[3] * dir.x + m[7] * dir.y + m[11] * dir.z;
	return Vec3(x / w, y / w, z / w);
}

string Mat4::ToString()
{
	ostringstream oss;
	oss << m[0] << "\t" << m[4] << "\t" << m[8] << "\t" << m[12] << endl;
	oss << m[1] << "\t" << m[5] << "\t" << m[9] << "\t" << m[13] << endl;
	oss << m[2] << "\t" << m[6] << "\t" << m[10] << "\t" << m[14] << endl;
	oss << m[3] << "\t" << m[7] << "\t" << m[11] << "\t" << m[15] << endl;
	return oss.str();
}

//Mat4& Mat4::operator*(const Mat4& rhs)
//{
//	return Multiply(rhs);
//}

Mat4& Mat4::SetPerspective(float fovy, float aspect, float zNear, float zFar)
{
	float cotangent, deltaZ;
	float radians = Mathf::ToRad(fovy / 2.0f);
	cotangent = cos(radians) / sin(radians);
	deltaZ = zNear - zFar;

	Get(0, 0) = cotangent / aspect;	Get(0, 1) = 0.0F;      Get(0, 2) = 0.0F;                    Get(0, 3) = 0.0F;
	Get(1, 0) = 0.0F;               Get(1, 1) = cotangent; Get(1, 2) = 0.0F;                    Get(1, 3) = 0.0F;
	Get(2, 0) = 0.0F;               Get(2, 1) = 0.0F;      Get(2, 2) = (zFar + zNear) / deltaZ; Get(2, 3) = 2.0F * zNear * zFar / deltaZ;
	Get(3, 0) = 0.0F;               Get(3, 1) = 0.0F;      Get(3, 2) = -1.0F;                   Get(3, 3) = 0.0F;

	return *this;
}


//---------------------------------------------------------
// static
Mat4 Mat4::Multiply(const Mat4& lmat, const Mat4& rmat)
{
	Mat4 result = lmat;
	return result.Multiply(rmat);
}

Mat4 Mat4::Translate(const Vec3& vec)
{
	Mat4 out;
	out.m[12] = vec.x;
	out.m[13] = vec.y;
	out.m[14] = vec.z;
	return out;
}

Mat4 Mat4::Translate(float x, float y, float z)
{
	return Mat4::Translate(Vec3(x, y, z));
}

Mat4 Mat4::Rotate(const Quaternion& q)
{
	return q.ToMat4();
}

Mat4 Mat4::Scale(const Vec3& vec)
{
	return Mat4::Scale(vec.x,vec.y,vec.z);
}

Mat4 Mat4::Scale(float sx, float sy, float sz)
{
	Mat4 out;
	out.m[0] = sx;
	out.m[5] = sy;
	out.m[10] = sz;
	return out;
}

Mat4 Mat4::TRS(const Vec3& pos, const Quaternion& q, const Vec3& scale)
{
	auto t = Mat4::Translate(pos);
	auto r = q.ToMat4();
	auto s = Mat4::Scale(scale);
	return  t * r * s;
}

Mat4 Mat4::Inverse(const Mat4& mat)
{
	// based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
	Mat4 out;
	float* te = out.m;
	const float* const me = mat.GetPtr();

	float n11 = me[0], n21 = me[1], n31 = me[2], n41 = me[3],
		n12 = me[4], n22 = me[5], n32 = me[6], n42 = me[7],
		n13 = me[8], n23 = me[9], n33 = me[10], n43 = me[11],
		n14 = me[12], n24 = me[13], n34 = me[14], n44 = me[15],

		t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
		t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
		t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
		t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;

	float det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;

	if (abs(det) < Mathf::kEpsilon) {
		MyLog::Info("Mat4::Inverse MayBe Error!\n");
		return  out;
	}

	float detInv = 1.0f / det;

	te[0] = t11 * detInv;
	te[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
	te[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
	te[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;

	te[4] = t12 * detInv;
	te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
	te[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
	te[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;

	te[8] = t13 * detInv;
	te[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
	te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
	te[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;

	te[12] = t14 * detInv;
	te[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
	te[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
	te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;

	return out;
}


//------------------
Mat4 operator*(const Mat4& lhs, const Mat4& rhs) {
	return Mat4(lhs).Multiply(rhs);
}